#! /usr/bin/env python

'''
    这是handler中的订阅者类，该类实现的目标是要接收订阅的消息并且按照要求进行解析
    同时调用core核心类来实现线程之间的切换
'''
import sys
sys.path.append("/home/galm/ms_01/src/command_receiver/scripts/query_controller")
from handlerCoreImpl import HandlerCoreImpl
from command_receiver.msg import roboCmd

class CommandSubscriber():

    def __init__(self):
        #要至少三个属性
        #一是记录当前属性的字符串
        #二是记录上一个属性的字符串，需要给默认值off
        #三是一个封装好各种消息的字典
        self.core = HandlerCoreImpl()
        self.tempCmd = roboCmd()
        self.curState = "off"
        self.preState = "off"
        self.cmdDir = {}

    '''
        这是用于解析指令的回调函数cmdParse
        这里需要封装信息到那个字典
        以及调用后一级handlerCore的方法来改变进程
    '''
    def cmdParse(self,rawCmd):
        #首先装载整个命令
        self.loadCmd(rawCmd)
        print(self.preState+"--->"+self.curState)
        #随后调用core中的方法执行命令
        if(self.curState == self.preState):
            #如果当前的状态和上一个状态相等，说明状态没有改变，这时就调用update函数直接更新该类的信息
            self.core.update(self.curState,self.cmdDir)
        else:
            #pass
            #如果当前状态和上一个状态不相等，那么就证明状态有变化，调用change函数切换进程
            self.core.change(self.preState,self.curState,self.cmdDir)

    '''
        self.state = "off" #当前状态是off
        self.navPos = [0,0,0] #当前的导航点
        self.carVel = [0,0] #当前车的前进速度和转向速度都是0
        self.armPos = [0,0] #当前机械臂的pitch和yaw都是0
        self.shoot = False #当前发射指令关
        self.readFlag = False #read是否进入


        string state
        float64 navPosX
        float64 navPosY
        float64 navPosZ
        int8 carVelLin
        int8 carVelRot
        int8 armPosPit
        int8 armPosYaw
        bool shoot
    '''

    def loadCmd(self,rawCmd):
        self.tempCmd = rawCmd
        #首先将当前状态变为上一个状态
        self.preState = self.curState
        self.curState = self.tempCmd.state
        self.cmdDir["navPos"] = [self.tempCmd.navPosX,self.tempCmd.navPosY,self.tempCmd.navPosZ]
        self.cmdDir["carVel"] = [self.tempCmd.carVelLin,self.tempCmd.carVelRot]
        self.cmdDir["armPos"] = [self.tempCmd.armPosPit,self.tempCmd.armPosYaw]
        self.cmdDir["shoot"] = self.tempCmd.shoot
        # print(self.cmdDir)

